// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: add dynamic type lookup service
// feature: support simple type discovery
// ------------------------------------------------------------------

#ifndef EDDS_CORE_PLICY__QOSPOLICIESSERIALIZER_HPP_
#define EDDS_CORE_PLICY__QOSPOLICIESSERIALIZER_HPP_

#include <ecdr/Cdr.h>
#include <edds/dds/core/policy/QosPolicies.hpp>
#include <edds/dds/core/policy/ParameterSerializer.hpp>

namespace evbs {
namespace edds {
namespace dds {

template <typename QosPolicy>
class QosPoliciesSerializer {
 public:
    static bool add_to_cdr_message(const QosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
        bool valid = ParameterSerializer<QosPolicy>::add_common_to_cdr_message(qos_policy, cdr_message);
        valid = add_content_to_cdr_message(qos_policy, cdr_message) && valid;
        return valid;
    }

    static bool read_from_cdr_message(QosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message,
                                      const uint16_t parameter_length) {
        bool valid;
        valid = read_content_from_cdr_message(qos_policy, cdr_message, parameter_length);
        return valid;
    }

    static uint32_t cdr_serialized_size(const QosPolicy& qos_policy) {
        return ParameterSerializer<QosPolicy>::cdr_serialized_size(qos_policy);
    }

 private:
    static bool add_content_to_cdr_message(const QosPolicy&, ertps::rtps::CDRMessage_t* const) {
        static_assert(sizeof(QosPolicy) == 0, "Not implemented");
        return false;
    }

    static bool read_content_from_cdr_message(QosPolicy&, ertps::rtps::CDRMessage_t* const, const uint16_t) {
        static_assert(sizeof(QosPolicy) == 0, "Not implemented");
        return false;
    }
};

template <>
inline bool QosPoliciesSerializer<DurabilityQosPolicy>::add_content_to_cdr_message(
    const DurabilityQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addOctet(cdr_message, qos_policy.kind);
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<DurabilityQosPolicy>::read_content_from_cdr_message(
    DurabilityQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_KIND_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readOctet(cdr_message, (ertps::rtps::octet*)&qos_policy.kind);
    cdr_message->pos += 3U;  // padding
    return valid;
}

template <>
inline bool QosPoliciesSerializer<DeadlineQosPolicy>::add_content_to_cdr_message(
    const DeadlineQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.period.seconds);
    valid = ertps::rtps::CDRMessage::addUInt32(cdr_message, qos_policy.period.fraction()) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<DeadlineQosPolicy>::read_content_from_cdr_message(
    DeadlineQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_TIME_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.period.seconds);
    uint32_t frac(0U);
    valid = ertps::rtps::CDRMessage::readUInt32(cdr_message, &frac) && valid;
    qos_policy.period.fraction(frac);
    return valid;
}

template <>
inline bool QosPoliciesSerializer<LivelinessQosPolicy>::add_content_to_cdr_message(
    const LivelinessQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addOctet(cdr_message, qos_policy.kind);
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.lease_duration.seconds) && valid;
    valid = ertps::rtps::CDRMessage::addUInt32(cdr_message, qos_policy.lease_duration.fraction()) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<LivelinessQosPolicy>::read_content_from_cdr_message(
    LivelinessQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_KIND_LENGTH + PARAMETER_TIME_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readOctet(cdr_message, (ertps::rtps::octet*)&qos_policy.kind);
    cdr_message->pos += 3U;  // padding
    valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.lease_duration.seconds) && valid;
    uint32_t frac(0U);
    valid = ertps::rtps::CDRMessage::readUInt32(cdr_message, &frac) && valid;
    qos_policy.lease_duration.fraction(frac);
    return valid;
}

template <>
inline bool QosPoliciesSerializer<ReliabilityQosPolicy>::add_content_to_cdr_message(
    const ReliabilityQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addOctet(cdr_message, qos_policy.kind);
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.max_blocking_time.seconds) && valid;
    valid = ertps::rtps::CDRMessage::addUInt32(cdr_message, qos_policy.max_blocking_time.fraction()) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<ReliabilityQosPolicy>::read_content_from_cdr_message(
    ReliabilityQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_KIND_LENGTH + PARAMETER_TIME_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readOctet(cdr_message, (ertps::rtps::octet*)&qos_policy.kind);
    cdr_message->pos += 3U;  // padding
    valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.max_blocking_time.seconds) && valid;
    uint32_t frac(0U);
    valid = ertps::rtps::CDRMessage::readUInt32(cdr_message, &frac) && valid;
    qos_policy.max_blocking_time.fraction(frac);
    return valid;
}

template <>
inline bool QosPoliciesSerializer<OwnershipQosPolicy>::add_content_to_cdr_message(
    const OwnershipQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addOctet(cdr_message, qos_policy.kind);
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<OwnershipQosPolicy>::read_content_from_cdr_message(
    OwnershipQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_KIND_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readOctet(cdr_message, (ertps::rtps::octet*)&qos_policy.kind);
    cdr_message->pos += 3U;  // padding
    return valid;
}

template <>
inline bool QosPoliciesSerializer<ResourceLimitsQosPolicy>::add_content_to_cdr_message(
    const ResourceLimitsQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.max_samples);
    valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.max_instances) && valid;
    valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.max_samples_per_instance) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<ResourceLimitsQosPolicy>::read_content_from_cdr_message(
    ResourceLimitsQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message,
    const uint16_t parameter_length) {
    if (parameter_length != 12U) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.max_samples);
    valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.max_instances) && valid;
    valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.max_samples_per_instance) && valid;
    return valid;
}

template <>
inline uint32_t QosPoliciesSerializer<PartitionQosPolicy>::cdr_serialized_size(const PartitionQosPolicy& qos_policy) {
    // p_id + p_length + partition_number
    uint32_t ret_val = 2U + 2U + 4U;
    for (PartitionQosPolicy::const_iterator it = qos_policy.begin(); it != qos_policy.end(); ++it) {
        // str_size
        ret_val += 4U;
        // str_data (including null char)
        ret_val += static_cast<uint32_t>(it->size());
        // align
        ret_val = (ret_val + 3U) & ~3U;
    }

    return ret_val;
}

template <>
inline bool QosPoliciesSerializer<PartitionQosPolicy>::add_to_cdr_message(
    const PartitionQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addUInt16(cdr_message, qos_policy.Pid);

    // Obtain qos_policy.length:
    uint16_t len = 4U;
    for (PartitionQosPolicy::const_iterator it = qos_policy.begin(); it != qos_policy.end(); ++it) {
        len += 4U;
        len += static_cast<uint16_t>(it->size());  // Already accounts for null char
        len = (len + 3U) & ~3U;
    }
    valid = ertps::rtps::CDRMessage::addUInt16(cdr_message, len) && valid;
    valid = ertps::rtps::CDRMessage::addUInt32(cdr_message, static_cast<uint32_t>(qos_policy.size())) && valid;
    for (PartitionQosPolicy::const_iterator it = qos_policy.begin(); it != qos_policy.end(); ++it) {
        valid &= ertps::rtps::CDRMessage::add_string(cdr_message, it->name());
    }

    return valid;
}

template <>
inline bool QosPoliciesSerializer<PartitionQosPolicy>::read_content_from_cdr_message(
    PartitionQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if ((qos_policy.max_size() != 0U) && (parameter_length > qos_policy.max_size() + 4U)) {
        return false;
    }
    qos_policy.length = parameter_length;

    uint32_t pos_ref = cdr_message->pos;
    uint32_t num_partitions = 0U;
    bool valid = ertps::rtps::CDRMessage::readUInt32(cdr_message, &num_partitions);
    // partitions_.reserve(parameter_length - 4);

    for (size_t i = 0U; i < num_partitions; ++i) {
        uint32_t partition_size, alignment;

        valid = ertps::rtps::CDRMessage::readUInt32(cdr_message, &partition_size) && valid;
        if (!valid) {
            return false;
        }

        qos_policy.push_back((const char*)&cdr_message->buffer[cdr_message->pos]);
        alignment = ((partition_size + 3U) & ~3U) - partition_size;
        cdr_message->pos += (partition_size + alignment);
    }
    // qos_policy.Npartitions_ = num_partitions;

    uint32_t length_diff = cdr_message->pos - pos_ref;
    valid = (parameter_length == length_diff) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<HistoryQosPolicy>::add_content_to_cdr_message(
    const HistoryQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addOctet(cdr_message, qos_policy.kind);
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, 0U) && valid;
    valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.depth) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<HistoryQosPolicy>::read_content_from_cdr_message(
    HistoryQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_KIND_LENGTH + 4U) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readOctet(cdr_message, (ertps::rtps::octet*)&qos_policy.kind);
    cdr_message->pos += 3U;  // padding
    valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.depth) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<LifespanQosPolicy>::add_content_to_cdr_message(
    const LifespanQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addInt32(cdr_message, qos_policy.duration.seconds);
    valid = ertps::rtps::CDRMessage::addUInt32(cdr_message, qos_policy.duration.fraction()) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<LifespanQosPolicy>::read_content_from_cdr_message(
    LifespanQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message, const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_TIME_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    bool valid = ertps::rtps::CDRMessage::readInt32(cdr_message, &qos_policy.duration.seconds);
    uint32_t frac(0U);
    valid = ertps::rtps::CDRMessage::readUInt32(cdr_message, &frac) && valid;
    qos_policy.duration.fraction(frac);
    return valid;
}

template <>
inline bool QosPoliciesSerializer<OwnershipStrengthQosPolicy>::add_content_to_cdr_message(
    const OwnershipStrengthQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addUInt32(cdr_message, qos_policy.value);
    return valid;
}

template <>
inline bool QosPoliciesSerializer<OwnershipStrengthQosPolicy>::read_content_from_cdr_message(
    OwnershipStrengthQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message,
    const uint16_t parameter_length) {
    if (parameter_length != 4U) {
        return false;
    }
    qos_policy.length = parameter_length;
    return ertps::rtps::CDRMessage::readUInt32(cdr_message, &qos_policy.value);
}

template <>
inline bool QosPoliciesSerializer<DisablePositiveACKsQosPolicy>::add_content_to_cdr_message(
    const DisablePositiveACKsQosPolicy&, ertps::rtps::CDRMessage_t* const cdr_message) {
    bool valid = ertps::rtps::CDRMessage::addOctet(cdr_message, (ertps::rtps::octet)0x01);
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, (ertps::rtps::octet)0x00) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, (ertps::rtps::octet)0x00) && valid;
    valid = ertps::rtps::CDRMessage::addOctet(cdr_message, (ertps::rtps::octet)0x00) && valid;
    return valid;
}

template <>
inline bool QosPoliciesSerializer<DisablePositiveACKsQosPolicy>::read_content_from_cdr_message(
    DisablePositiveACKsQosPolicy& qos_policy, ertps::rtps::CDRMessage_t* const cdr_message,
    const uint16_t parameter_length) {
    if (parameter_length != PARAMETER_BOOL_LENGTH) {
        return false;
    }
    qos_policy.length = parameter_length;
    ertps::rtps::octet value(0U);
    bool valid = ertps::rtps::CDRMessage::readOctet(cdr_message, &value);
    qos_policy.enabled = (value == 0U) ? false : true;
    cdr_message->pos += 3U;  // padding
    return valid;
}

}  // namespace dds
}  // namespace edds
}  // namespace evbs

#endif  // EDDS_CORE_PLICY__QOSPOLICIESSERIALIZER_HPP_
